import smbus
import time
import serial
import logging


class PCA9685:
    # Registers/etc.
    __SUBADR1 = 0x02
    __SUBADR2 = 0x03
    __SUBADR3 = 0x04
    __MODE1 = 0x00
    __PRESCALE = 0xFE
    __LED0_ON_L = 0x06
    __LED0_ON_H = 0x07
    __LED0_OFF_L = 0x08
    __LED0_OFF_H = 0x09

    def __init__(self, address, debug=False):
        self.bus = smbus.SMBus(1)
        self.address = address
        self.debug = debug
        self.logger = logging.getLogger('PCA9685')
        self.last_angles = {}  # 存储各通道最后设置的角度

        if debug:
            self.logger.setLevel(logging.DEBUG)
        else:
            self.logger.setLevel(logging.INFO)

        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        ch = logging.StreamHandler()
        ch.setFormatter(formatter)
        self.logger.addHandler(ch)

        self.set_pwm_freq(50)

    def write(self, reg, value):
        self.bus.write_byte_data(self.address, reg, value)
        if self.debug:
            self.logger.debug(f"I2C: Write 0x{value:02X} to register 0x{reg:02X}")

    def read(self, reg):
        result = self.bus.read_byte_data(self.address, reg)
        if self.debug:
            self.logger.debug(f"I2C: Device 0x{self.address:02X} returned 0x{result:02X} from reg 0x{reg:02X}")
        return result

    def set_pwm_freq(self, freq):
        prescaleval = 25000000.0 / 4096.0 / float(freq) - 1.0
        prescale = int(prescaleval + 0.5)
        oldmode = self.read(self.__MODE1)
        self.write(self.__MODE1, (oldmode & 0x7F) | 0x10)
        self.write(self.__PRESCALE, prescale)
        self.write(self.__MODE1, oldmode)
        time.sleep(0.005)
        self.write(self.__MODE1, oldmode | 0x01)
        self.logger.info(f"Set PWM frequency to {freq} Hz")

    def set_pwm(self, channel, on, off):
        offset = channel * 4
        self.write(self.__LED0_ON_L + offset, on & 0xFF)
        self.write(self.__LED0_ON_H + offset, on >> 8)
        self.write(self.__LED0_OFF_L + offset, off & 0xFF)
        self.write(self.__LED0_OFF_H + offset, off >> 8)
        if self.debug:
            self.logger.debug(f"Set PWM for channel {channel}: ON={on}, OFF={off}")

    def set_servo_angle(self, channel, angle):
        """优化：仅当角度变化超过1度时才更新PWM"""
        if channel in self.last_angles and abs(angle - self.last_angles[channel]) < 1.0:
            return

        pulse_width_us = (angle / 180.0) * (2500 - 500) + 500
        pulse = int(pulse_width_us * 4096 / 20000)
        self.set_pwm(channel, 0, pulse)
        self.last_angles[channel] = angle  # 记录当前角度
        time.sleep(0.02)  # 缩短延时但仍保证舵机响应
        self.logger.info(f"Set servo angle for channel {channel}: {angle} degrees")


class PanTiltController:
    def __init__(self, horizontal_min, horizontal_max, vertical_min, vertical_max, center_threshold,
                 kp_pan, ki_pan, kp_tilt, ki_tilt, alpha, camera_flipped=False, debug=False):
        self.pca9685 = PCA9685(0x40, debug)
        self.horizontal_min = horizontal_min
        self.horizontal_max = horizontal_max
        self.vertical_min = vertical_min
        self.vertical_max = vertical_max
        self.center_threshold = center_threshold
        self.kp_pan = kp_pan
        self.ki_pan = ki_pan
        self.kp_tilt = kp_tilt
        self.ki_tilt = ki_tilt
        self.alpha = alpha
        self.integral_x = 0
        self.integral_y = 0
        self.current_pan = 90
        self.current_tilt = 90
        self.camera_flipped = camera_flipped
        self.last_update_time = time.time()
        self.idle_timeout = 2.0  # 无操作超时时间（秒）

        self.logger = logging.getLogger('PanTiltController')
        if debug:
            self.logger.setLevel(logging.DEBUG)
        else:
            self.logger.setLevel(logging.INFO)

        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        ch = logging.StreamHandler()
        ch.setFormatter(formatter)
        self.logger.addHandler(ch)

        # 摄像头图像尺寸(假设为QVGA 320x240)
        self.image_width = 320
        self.image_height = 240

    def flip_coordinates(self, x, y):
        """翻转坐标以适应倒置的摄像头"""
        if not self.camera_flipped:
            return x, y

        # 水平翻转
        flipped_x = self.image_width - x
        # 垂直翻转
        flipped_y = self.image_height - y
        # 原点移至中心
        center_x = self.image_width // 2
        center_y = self.image_height // 2

        return flipped_x - center_x, flipped_y - center_y

    def parse_data(self, raw_data):
        try:
            data = raw_data.decode('utf-8', errors='ignore').strip()
            self.logger.debug(f"Received data: {data}")
            if data.startswith('$') and data.endswith('#'):
                coords = data[1:-1].split(',')
                if len(coords) == 2:
                    try:
                        x = int(coords[0])
                        y = int(coords[1])

                        # 将坐标转换为相对于图像中心的值
                        center_x = self.image_width // 2
                        center_y = self.image_height // 2
                        x_relative = x - center_x
                        y_relative = center_y - y  # y轴方向调整

                        # 应用坐标翻转
                        x_flipped, y_flipped = self.flip_coordinates(x_relative, y_relative)

                        return x_flipped, y_flipped
                    except ValueError as ve:
                        self.logger.error(f"Failed to convert to integer: {ve}, Data: {coords}")
                else:
                    self.logger.error("Incorrect data format, missing two values separated by comma")
            else:
                self.logger.error("Incorrect data format, missing $ at the beginning or # at the end")
        except Exception as e:
            self.logger.error(f"Data parsing failed: {e}, Raw data: {raw_data}")
        return None, None

    def control(self):
        ser = serial.Serial(
            port='/dev/ttyS0',
            baudrate=9600,
            timeout=1,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE,
            bytesize=serial.EIGHTBITS
        )
        try:
            while True:
                current_time = time.time()
                has_data = False

                if ser.in_waiting > 0:
                    buffer = ser.read(ser.in_waiting)
                    if b'#' in buffer:
                        parts = buffer.split(b'#', 1)
                        raw_data = parts[0] + b'#'
                        x, y = self.parse_data(raw_data)
                        if x is not None and y is not None:
                            has_data = True
                            self.last_update_time = current_time

                            # 应用低通滤波
                            x_filtered = self.alpha * x + (1 - self.alpha) * 0
                            y_filtered = self.alpha * y + (1 - self.alpha) * 0

                            # 水平控制（带死区检测）
                            if abs(x_filtered) > self.center_threshold:
                                self.integral_x += x_filtered
                                self.integral_x = max(min(self.integral_x, 500), -500)
                                pan_adjust = x_filtered * self.kp_pan + self.integral_x * self.ki_pan
                                new_pan = self.current_pan - pan_adjust
                                new_pan = max(min(new_pan, self.horizontal_max), self.horizontal_min)
                                if abs(new_pan - self.current_pan) >= 1.0:  # 变化超过1度才更新
                                    self.current_pan = new_pan
                                    self.pca9685.set_servo_angle(9, self.current_pan)
                                    self.logger.info(f"Pan adjusted to: {self.current_pan:.1f}")

                            # 垂直控制（带死区检测）
                            if abs(y_filtered) > self.center_threshold:
                                self.integral_y += y_filtered
                                self.integral_y = max(min(self.integral_y, 500), -500)
                                tilt_adjust = y_filtered * self.kp_tilt + self.integral_y * self.ki_tilt
                                new_tilt = self.current_tilt + tilt_adjust
                                new_tilt = max(min(new_tilt, self.vertical_max), self.vertical_min)
                                if abs(new_tilt - self.current_tilt) >= 1.0:  # 变化超过1度才更新
                                    self.current_tilt = new_tilt
                                    self.pca9685.set_servo_angle(10, self.current_tilt)
                                    self.logger.info(f"Tilt adjusted to: {self.current_tilt:.1f}")

                # 无数据时的归中逻辑（带超时检测）
                if not has_data and (current_time - self.last_update_time) > self.idle_timeout:
                    if abs(self.current_pan - 90) > 1.0 or abs(self.current_tilt - 90) > 1.0:
                        delta_pan = (90 - self.current_pan) * 0.2  # 降低归中速度
                        delta_tilt = (90 - self.current_tilt) * 0.2
                        self.current_pan += delta_pan
                        self.current_tilt += delta_tilt
                        self.pca9685.set_servo_angle(9, int(self.current_pan))
                        self.pca9685.set_servo_angle(10, int(self.current_tilt))
                        self.logger.debug(f"Homing: Pan={int(self.current_pan)}, Tilt={int(self.current_tilt)}")

                time.sleep(0.05)  # 主循环固定延时

        except KeyboardInterrupt:
            self.logger.info("Program terminated")
        finally:
            # 关机时重置所有通道
            for ch in range(16):
                self.pca9685.set_pwm(ch, 0, 0)


if __name__ == "__main__":
    horizontal_min = 20
    horizontal_max = 160
    vertical_min = 30
    vertical_max = 150
    center_threshold = 15
    kp_pan = 1.0
    ki_pan = 2.0
    kp_tilt = 1.0
    ki_tilt = 0.01
    alpha = 0.3
    camera_flipped = True  # 设置为True表示摄像头倒置

    controller = PanTiltController(horizontal_min, horizontal_max, vertical_min, vertical_max, center_threshold,
                                   kp_pan, ki_pan, kp_tilt, ki_tilt, alpha, camera_flipped, debug=True)
    controller.control()
